/**
 * @author Russell Toris - rctoris@wpi.edu
 */

var ROS2D = ROS2D || {
  REVISION: '0.9.0'
};

// convert the given global Stage coordinates to ROS coordinates
createjs.Stage.prototype.globalToRos = function (x, y) {
  //console.log("ros2d.js-11-globalToRos");
  //console.log("ros2d.js-12-x: " + x);
  //console.log("ros2d.js-14-y: " + y);
  //console.log("ros2d.js-15-this.x: " + this.x);
  //console.log("ros2d.js-16-this.y: " + this.y);
  //console.log("ros2d.js-17-this.scaleX: " + this.scaleX);
  //console.log("ros2d.js-18-this.scaleY: " + this.scaleY);
  var rosX = (x - this.x) / this.scaleX;
  var rosY = (this.y - y) / this.scaleY;
  //console.log("ros2d.js-20-rosX: " + rosX);
  //console.log("ros2d.js-21-rosY: " + rosY);
  return new ROSLIB.Vector3({
    x: rosX,
    y: rosY
  });
};

// convert the given ROS coordinates to global Stage coordinates
createjs.Stage.prototype.rosToGlobal = function (pos) {
  //console.log("ros2d.js-22-rosToGlobal");
  var x = pos.x * this.scaleX + this.x;
  var y = pos.y * this.scaleY + this.y;
  return {
    x: x,
    y: y
  };
};

// convert a ROS quaternion to theta in degrees
createjs.Stage.prototype.rosQuaternionToGlobalTheta = function (orientation) {
  //console.log("ros2d.js-33-rosQuaternionToGlobalTheta");
  // See https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles#Rotation_matrices
  // here we use [x y z] = R * [1 0 0]
  var q0 = orientation.w;
  var q1 = orientation.x;
  var q2 = orientation.y;
  var q3 = orientation.z;
  // Canvas rotation is clock wise and in degrees
  return -Math.atan2(2 * (q0 * q3 + q1 * q2), 1 - 2 * (q2 * q2 + q3 * q3)) * 180.0 / Math.PI;
};
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * An image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
 *
 * @constructor
 * @param options - object with following keys:
 *   * message - the occupancy grid map meta data message
 *   * image - the image URL to load
 */
ROS2D.ImageMap = function (options) {
  //console.log("ros2d.js-57-ImageMap");
  options = options || {};
  var message = options.message;
  var image = options.image;

  // save the metadata we need
  this.pose = new ROSLIB.Pose({
    position: message.origin.position,
    orientation: message.origin.orientation
  });

  // set the size
  this.width = message.width;
  this.height = message.height;

  // create the bitmap
  createjs.Bitmap.call(this, image);
  // change Y direction
  this.y = -this.height * message.resolution;

  // scale the image
  this.scaleX = message.resolution;
  this.scaleY = message.resolution;
  this.width *= this.scaleX;
  this.height *= this.scaleY;

  // set the pose
  this.x += this.pose.position.x;
  this.y -= this.pose.position.y;
};
ROS2D.ImageMap.prototype.__proto__ = createjs.Bitmap.prototype;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A image map is a PNG image scaled to fit to the dimensions of a OccupancyGrid.
 *
 * Emits the following events:
 *   * 'change' - there was an update or change in the map
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * topic (optional) - the map meta data topic to listen to
 *   * image - the image URL to load
 *   * rootObject (optional) - the root object to add this marker to
 */
ROS2D.ImageMapClient = function (options) {
  //console.log("ros2d.js-107-ImageMapClient");
  var that = this;
  options = options || {};
  var ros = options.ros;
  var topic = options.topic || '/map_metadata';
  this.image = options.image;
  this.rootObject = options.rootObject || new createjs.Container();

  // create an empty shape to start with
  this.currentImage = new createjs.Shape();

  // subscribe to the topic
  var rosTopic = new ROSLIB.Topic({
    ros: ros,
    name: topic,
    messageType: 'nav_msgs/msg/MapMetaData'
  });

  rosTopic.subscribe(function (message) {
    // we only need this once
    rosTopic.unsubscribe();

    // create the image
    that.currentImage = new ROS2D.ImageMap({
      message: message,
      image: that.image
    });
    that.rootObject.addChild(that.currentImage);
    // work-around for a bug in easeljs -- needs a second object to render correctly
    that.rootObject.addChild(new ROS2D.Grid({ size: 1 }));

    that.emit('change');
  });
};
ROS2D.ImageMapClient.prototype.__proto__ = EventEmitter2.prototype;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * An OccupancyGrid can convert a ROS occupancy grid message into a createjs Bitmap object.
 *
 * @constructor
 * @param options - object with following keys:
 *   * message - the occupancy grid message
 */
ROS2D.OccupancyGrid = function (options) {
  //console.log("ros2d.js-155-OccupancyGrid");
  options = options || {};
  var message = options.message;

  // internal drawing canvas
  var canvas = document.createElement('canvas');
  var context = canvas.getContext('2d');

  // save the metadata we need
  this.pose = new ROSLIB.Pose({
    position: message.info.origin.position,
    orientation: message.info.origin.orientation
  });

  // set the size
  this.width = message.info.width;
  this.height = message.info.height;
  canvas.width = this.width;
  canvas.height = this.height;

  var imageData = context.createImageData(this.width, this.height);
  for (var row = 0; row < this.height; row++) {
    for (var col = 0; col < this.width; col++) {
      // determine the index into the map data
      var mapI = col + ((this.height - row - 1) * this.width);
      // determine the value
      var data = message.data[mapI];
      var val;
      if (data === 100) {
        val = 0;
      } else if (data === 0) {
        val = 255;
      } else {
        val = 127;
      }

      // determine the index into the image data array
      var i = (col + (row * this.width)) * 4;
      // r
      imageData.data[i] = val;
      // g
      imageData.data[++i] = val;
      // b
      imageData.data[++i] = val;
      // a
      imageData.data[++i] = 255;
    }
  }
  context.putImageData(imageData, 0, 0);

  // create the bitmap
  createjs.Bitmap.call(this, canvas);
  // change Y direction
  this.y = -this.height * message.info.resolution;

  // scale the image
  this.scaleX = message.info.resolution;
  this.scaleY = message.info.resolution;
  this.width *= this.scaleX;
  this.height *= this.scaleY;

  // set the pose
  this.x += this.pose.position.x;
  this.y -= this.pose.position.y;
};
ROS2D.OccupancyGrid.prototype.__proto__ = createjs.Bitmap.prototype;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A map that listens to a given occupancy grid topic.
 *
 * Emits the following events:
 *   * 'change' - there was an update or change in the map
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * topic (optional) - the map topic to listen to
 *   * rootObject (optional) - the root object to add this marker to
 *   * continuous (optional) - if the map should be continuously loaded (e.g., for SLAM)
 */
ROS2D.OccupancyGridClient = function (options) {
  //console.log("ros2d.js-240-OccupancyGridClient");
  var that = this;
  options = options || {};
  var ros = options.ros;
  var topic = options.topic || '/map';
  this.continuous = options.continuous;
  this.rootObject = options.rootObject || new createjs.Container();

  // current grid that is displayed
  // create an empty shape to start with, so that the order remains correct.
  this.currentGrid = new createjs.Shape();
  this.rootObject.addChild(this.currentGrid);
  // work-around for a bug in easeljs -- needs a second object to render correctly
  this.rootObject.addChild(new ROS2D.Grid({ size: 1 }));

  // subscribe to the topic
  var rosTopic = new ROSLIB.Topic({
    ros: ros,
    name: topic,
    messageType: 'nav_msgs/msg/OccupancyGrid',
    compression: 'png'
  });

  rosTopic.subscribe(function (message) {
    // check for an old map
    var index = null;
    if (that.currentGrid) {
      index = that.rootObject.getChildIndex(that.currentGrid);
      that.rootObject.removeChild(that.currentGrid);
    }

    that.currentGrid = new ROS2D.OccupancyGrid({
      message: message
    });
    if (index !== null) {
      that.rootObject.addChildAt(that.currentGrid, index);
    }
    else {
      that.rootObject.addChild(that.currentGrid);
    }

    that.emit('change');

    // check if we should unsubscribe
    if (!that.continuous) {
      rosTopic.unsubscribe();
    }
  });
};
ROS2D.OccupancyGridClient.prototype.__proto__ = EventEmitter2.prototype;
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Jihoon Lee- jihoonlee.in@gmail.com
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A static map that receives from map_server.
 *
 * Emits the following events:
 *   * 'change' - there was an update or change in the map
 *
 * @constructor
 * @param options - object with following keys:
 *   * ros - the ROSLIB.Ros connection handle
 *   * service (optional) - the map topic to listen to, like '/static_map'
 *   * rootObject (optional) - the root object to add this marker to
 */
ROS2D.OccupancyGridSrvClient = function (options) {
  //console.log("ros2d.js-309-OccupancyGridSrvClient");
  var that = this;
  options = options || {};
  var ros = options.ros;
  var service = options.service || '/static_map';
  this.rootObject = options.rootObject || new createjs.Container();

  // current grid that is displayed
  this.currentGrid = null;

  // Setting up to the service
  var rosService = new ROSLIB.Service({
    ros: ros,
    name: service,
    serviceType: 'nav_msgs/msg/GetMap',
    compression: 'png'
  });

  rosService.callService(new ROSLIB.ServiceRequest(), function (response) {
    // check for an old map
    if (that.currentGrid) {
      that.rootObject.removeChild(that.currentGrid);
    }

    that.currentGrid = new ROS2D.OccupancyGrid({
      message: response.map
    });
    that.rootObject.addChild(that.currentGrid);

    that.emit('change', that.currentGrid);
  });
};
ROS2D.OccupancyGridSrvClient.prototype.__proto__ = EventEmitter2.prototype;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * An arrow with line and triangular head, based on the navigation arrow.
 * Aims to the left at 0 rotation, as would be expected.
 *
 * @constructor
 * @param options - object with following keys:
 *   * size (optional) - the size of the marker
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 *   * fillColor (optional) - the createjs color for the fill
 *   * pulse (optional) - if the marker should "pulse" over time
 */
ROS2D.ArrowShape = function (options) {
  console.log("ros2d.js-360-ArrowShape");
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var strokeSize = options.strokeSize || 3;
  var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
  var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
  var pulse = options.pulse;

  // draw the arrow
  var graphics = new createjs.Graphics();

  var headLen = size / 3.0;
  var headWidth = headLen * 2.0 / 3.0;

  graphics.setStrokeStyle(strokeSize);
  graphics.beginStroke(strokeColor);
  graphics.moveTo(0, 0);
  graphics.lineTo(size - headLen, 0);

  graphics.beginFill(fillColor);
  graphics.moveTo(size, 0);
  graphics.lineTo(size - headLen, headWidth / 2.0);
  graphics.lineTo(size - headLen, -headWidth / 2.0);
  graphics.closePath();
  graphics.endFill();
  graphics.endStroke();

  // create the shape
  createjs.Shape.call(this, graphics);

  // check if we are pulsing
  if (pulse) {
    // have the model "pulse"
    var growCount = 0;
    var growing = true;
    createjs.Ticker.addEventListener('tick', function () {
      if (growing) {
        that.scaleX *= 1.035;
        that.scaleY *= 1.035;
        growing = (++growCount < 10);
      } else {
        that.scaleX /= 1.035;
        that.scaleY /= 1.035;
        growing = (--growCount < 0);
      }
    });
  }
};
ROS2D.ArrowShape.prototype.__proto__ = createjs.Shape.prototype;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Raffaello Bonghi - raffaello.bonghi@officinerobotiche.it
 */

/**
 * A Grid object draw in map.
 *
 * @constructor
 * @param options - object with following keys:
 *  * size (optional) - the size of the grid
 *  * cellSize (optional) - the cell size of map
 *  * lineWidth (optional) - the width of the lines in the grid
 */
ROS2D.Grid = function (options) {
  console.log("ros2d.js-425-Grid");
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var cellSize = options.cellSize || 0.1;
  var lineWidth = options.lineWidth || 0.001;
  // draw the arrow
  var graphics = new createjs.Graphics();
  // line width
  graphics.setStrokeStyle(lineWidth * 5);
  graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
  graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
  graphics.moveTo(-size * cellSize, 0);
  graphics.lineTo(size * cellSize, 0);
  graphics.moveTo(0, -size * cellSize);
  graphics.lineTo(0, size * cellSize);
  graphics.endFill();
  graphics.endStroke();

  graphics.setStrokeStyle(lineWidth);
  graphics.beginStroke(createjs.Graphics.getRGB(0, 0, 0));
  graphics.beginFill(createjs.Graphics.getRGB(255, 0, 0));
  for (var i = -size; i <= size; i++) {
    graphics.moveTo(-size * cellSize, i * cellSize);
    graphics.lineTo(size * cellSize, i * cellSize);
    graphics.moveTo(i * cellSize, -size * cellSize);
    graphics.lineTo(i * cellSize, size * cellSize);
  }
  graphics.endFill();
  graphics.endStroke();
  // create the shape
  createjs.Shape.call(this, graphics);

};
ROS2D.Grid.prototype.__proto__ = createjs.Shape.prototype;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A navigation arrow is a directed triangle that can be used to display orientation.
 *
 * @constructor
 * @param options - object with following keys:
 *   * size (optional) - the size of the marker
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 *   * fillColor (optional) - the createjs color for the fill
 *   * pulse (optional) - if the marker should "pulse" over time
 */
ROS2D.NavigationArrow = function (options) {
  console.log("ros2d.js-477-NavigationArrow");
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var strokeSize = options.strokeSize || 3;
  var strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
  var fillColor = options.fillColor || createjs.Graphics.getRGB(255, 0, 0);
  var pulse = options.pulse;

  // draw the arrow
  var graphics = new createjs.Graphics();
  // line width
  graphics.setStrokeStyle(strokeSize);

  graphics.beginStroke(strokeColor);
  graphics.beginFill(fillColor);
  graphics.moveTo(0, 0);
  graphics.lineTo(-size / 2.0, size / 2.0);
  graphics.lineTo(size, 0.38);
  graphics.lineTo(0, 0)
  graphics.lineTo(-size / 2.0, -size / 2.0);
  graphics.lineTo(size, -0.38);

  graphics.closePath();
  graphics.endFill();
  graphics.endStroke();

  // create the shape
  createjs.Shape.call(this, graphics);

  // check if we are pulsing
  if (pulse) {
    // have the model "pulse"
    var growCount = 0;
    var growing = true;
    createjs.Ticker.addEventListener('tick', function () {
      if (growing) {
        that.scaleX *= 1.035;
        that.scaleY *= 1.035;
        growing = (++growCount < 10);
      } else {
        that.scaleX /= 1.035;
        that.scaleY /= 1.035;
        growing = (--growCount < 0);
      }
    });
  }
};
ROS2D.NavigationArrow.prototype.__proto__ = createjs.Shape.prototype;
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Inigo Gonzalez - ingonza85@gmail.com
 */

/**
 * A navigation image that can be used to display orientation.
 *
 * @constructor
 * @param options - object with following keys:
 *   * size (optional) - the size of the marker
 *   * image - the image to use as a marker
 *   * pulse (optional) - if the marker should "pulse" over time
 */
ROS2D.NavigationImage = function (options) {
  console.log("ros2d.js-536-NavigationImage");
  var that = this;
  options = options || {};
  var size = options.size || 10;
  var image_url = options.image;
  var pulse = options.pulse;
  var alpha = options.alpha || 1;

  var originals = {};

  var paintImage = function () {
    createjs.Bitmap.call(that, image);
    var scale = calculateScale(size);
    that.alpha = alpha;
    that.scaleX = scale;
    that.scaleY = scale;
    that.regY = that.image.height / 2;
    that.regX = that.image.width / 2;
    originals['rotation'] = that.rotation;
    Object.defineProperty(that, 'rotation', {
      get: function () { return originals['rotation'] + 90; },
      set: function (value) { originals['rotation'] = value; }
    });
    if (pulse) {
      // have the model "pulse"
      var growCount = 0;
      var growing = true;
      var SCALE_SIZE = 1.020;
      createjs.Ticker.addEventListener('tick', function () {
        if (growing) {
          that.scaleX *= SCALE_SIZE;
          that.scaleY *= SCALE_SIZE;
          growing = (++growCount < 10);
        } else {
          that.scaleX /= SCALE_SIZE;
          that.scaleY /= SCALE_SIZE;
          growing = (--growCount < 0);
        }
      });
    }
  };

  var calculateScale = function (_size) {
    return _size / image.width;
  };

  var image = new Image();
  image.onload = paintImage;
  image.src = image_url;

};

ROS2D.NavigationImage.prototype.__proto__ = createjs.Bitmap.prototype;
////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * A shape to draw a nav_msgs/Path msg
 *
 * @constructor
 * @param options - object with following keys:
 *   * path (optional) - the initial path to draw
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 */
ROS2D.PathShape = function (options) {
  //console.log("ros2d.js-604-PathShape");
  options = options || {};
  var path = options.path;
  this.strokeSize = options.strokeSize || 3;
  this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);

  // draw the line
  this.graphics = new createjs.Graphics();

  if (path !== null && typeof path !== 'undefined') {
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
    this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
    for (var i = 1; i < path.poses.length; ++i) {
      this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
    }
    this.graphics.endStroke();
  }

  // create the shape
  createjs.Shape.call(this, this.graphics);
};

/**
 * Set the path to draw
 *
 * @param path of type nav_msgs/Path
 */
ROS2D.PathShape.prototype.setPath = function (path) {
  //console.log("ros2d.js-633-setPath");
  this.graphics.clear();
  if (path !== null && typeof path !== 'undefined') {
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
    this.graphics.moveTo(path.poses[0].pose.position.x / this.scaleX, path.poses[0].pose.position.y / -this.scaleY);
    for (var i = 1; i < path.poses.length; ++i) {
      this.graphics.lineTo(path.poses[i].pose.position.x / this.scaleX, path.poses[i].pose.position.y / -this.scaleY);
    }
    this.graphics.endStroke();
  }
};
ROS2D.PathShape.prototype.__proto__ = createjs.Shape.prototype;

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * A polygon that can be edited by an end user
 *
 * @constructor
 * @param options - object with following keys:
 *   * pose (optional) - the first pose of the trace
 *   * lineSize (optional) - the width of the lines
 *   * lineColor (optional) - the createjs color of the lines
 *   * pointSize (optional) - the size of the points
 *   * pointColor (optional) - the createjs color of the points
 *   * fillColor (optional) - the createjs color to fill the polygon
 *   * lineCallBack (optional) - callback function for mouse interaction with a line
 *   * pointCallBack (optional) - callback function for mouse interaction with a point
 */
ROS2D.PolygonMarker = function (options) {
  //console.log("ros2d.js-667-PolygonMarker");
  //	var that = this;
  options = options || {};
  this.lineSize = options.lineSize || 0.05;
  this.lineColor = options.lineColor || createjs.Graphics.getRGB(0, 0, 255, 100);        // 蓝色
  this.pointSize = options.pointSize || 0.05;
  this.pointColor = options.pointColor || createjs.Graphics.getRGB(255, 0, 0, 0.66);      //红色
  // this.fillColor = options.pointColor || createjs.Graphics.getRGB(0, 255, 0, 0.33);
  this.lineCallBack = options.lineCallBack;
  this.pointCallBack = options.pointCallBack;
  this.curveCallBack = options.curveCallBack;

  this.curvesObj = [];          //数组

  // Array of point shapes
  this.pointContainer = new createjs.Container();             // Container是一个容器，内部元素可以相对移动
  // Array of line shapes
  this.lineContainer = new createjs.Container();              //点，线，贝塞尔控制点容器
  // Array of bezier
  this.bezierCPointContainer = new createjs.Container();

  this.fillShape = new createjs.Shape();          //shape图形对象

  // Container with all the lines and points
  createjs.Container.call(this);

  this.addChild(this.fillShape);
  this.addChild(this.lineContainer);
  this.addChild(this.pointContainer);
  this.addChild(this.bezierCPointContainer)
};

ROS2D.PolygonMarker.prototype.rotationJudge = function (angle) {  //贝塞尔角度
  console.log("ros2d.js-1008-rotationJudge");
  var ang = angle;
  if (ang != 0 || ang != 180) {
    ang = -ang;
  }
  return ang;
};

//get line json data
ROS2D.PolygonMarker.prototype.getCurvesObj = function () {    //获取线的json数据
  console.log("ros2d.js-1001-getCurvesObj");
  // console.log(this.curvesObj);
  return this.curvesObj;
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////


///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//贝塞尔曲线后退属性
ROS2D.PolygonMarker.prototype.createBackBezierCtpiont = function (startPoint, endPoint, type) {
  console.log("createBackBezierCtpiont");
  var cpx;
  var cpy;
  var startyaw = this.rotationJudge(startPoint.rotation);   //开始角度
  var endyaw = this.rotationJudge(endPoint.rotation);       //结束角度
  //贝塞尔曲线长度
  var length = Math.sqrt((startPoint.x - endPoint.x) * (startPoint.x - endPoint.x) + (startPoint.y - endPoint.y) * (startPoint.y - endPoint.y));
  if (type === 0) {     //parseFloat() 函数可解析一个字符串，并返回一个浮点数。
    cpx = parseFloat(startPoint.x) + length * Math.cos(parseFloat(startyaw) * 0.017453293) * 2 / 3;     //1开始控制点
    cpy = parseFloat(startPoint.y) - length * Math.sin(parseFloat(startyaw) * 0.017453293) * 2 / 3;
  } else {
    cpx = parseFloat(endPoint.x) - length * Math.cos(parseFloat(endyaw) * 0.017453293) * 2 / 3;         //0结束控制点
    cpy = parseFloat(endPoint.y) + length * Math.sin(parseFloat(endyaw) * 0.017453293) * 2 / 3;
  }
  return new ROSLIB.Vector3({
    x: cpx,
    y: -cpy,
  });
};
ROS2D.PolygonMarker.prototype.createBackBezierText = function (startPoint, endPoint, cp1, cp2, velocity, numpath) {
  console.log("createBackBezierText");
  var pathlist = this.getCurvesObj();
  var numPaths = this.lineContainer.getNumChildren() / 2;
  var pathName = new createjs.Text(numPaths + 1, "0.2px Arial", "#0000ff");   //已有路径数量+1
  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;     //贝塞尔曲线的x，y坐标值与名字
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
  pathName.name = "bezier";
  var startyaw = this.rotationJudge(-startPoint.rotation);
  var endyaw = this.rotationJudge(-endPoint.rotation);
  var item = {
    index: numPaths + 1,
    mode: pathName.name,
    velocity: velocity,
    segment: "backward",
    startPoint: { num: parseInt(numpath)+1, x: startPoint.x, y: startPoint.y, rotation: startyaw },
    endPoint: { num: parseInt(numpath), x: endPoint.x, y: endPoint.y, rotation: endyaw },
    cp1: { x: cp1.x, y: -cp1.y },
    cp2: { x: cp2.x, y: -cp2.y }
  };
  this.curvesObj.push(item);    //将贝塞尔信息加入curvesObj数组
  return pathName;
};
//创建贝塞尔曲线形状
ROS2D.PolygonMarker.prototype.createBackBezierShape = function (startPoint, endPoint, cp1, cp2) {
  console.log("createBackBezierShape");
  var bezier = new createjs.Shape();                                //创建一个形状的显示对象
  this.editBackBezierShape(bezier, startPoint, endPoint, cp1, cp2);     //调用editBezierShape函数
  var that = this;
  bezier.addEventListener('mousedown', function (event) {           //事件监听
    if (that.curveCallBack !== null && typeof that.curveCallBack !== 'undefined') {
      that.curveCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
    }
  });
  return bezier;
};
//编辑贝塞尔形状
ROS2D.PolygonMarker.prototype.editBackBezierShape = function (bezier, startPoint, endPoint, cp1, cp2) {
  console.log("editBackBezierShape");
  bezier.graphics.clear();                                //clear()清除所有绘制命令,重置所有绘制设置
  bezier.graphics.setStrokeStyle(this.lineSize);          //设置线条样式
  bezier.graphics.beginStroke(this.lineColor);            //beginStroke() 指定绘制线条的样式(颜色)
  bezier.graphics.moveTo(startPoint.x, -startPoint.y);     //绘制直线，直线的起始点
  bezier.graphics.bezierCurveTo(cp1.x, cp1.y, cp2.x, cp2.y, endPoint.x, -endPoint.y);      //绘制贝塞尔曲线
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////


////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//闭合前进曲线文本
ROS2D.PolygonMarker.prototype.createBezierEndText = function (startPoint, endPoint, cp1, cp2, velocity) {
  console.log("createBezierEndText");
  var numPoints = this.pointContainer.getNumChildren() / 2;
  console.log("目前点数量 "+numPoints);     //点的数量
  var numPaths = this.lineContainer.getNumChildren() / 2;
  console.log("目前路径数量 "+numPaths);    //线的数量
  var pathName = new createjs.Text(numPaths + 1, "0.2px Arial", "#0000ff");   //已有路径数量+1
  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;     //贝塞尔曲线的x，y坐标值与名字
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
  pathName.name = "bezier";
  var startyaw = this.rotationJudge(startPoint.rotation);
  var endyaw = this.rotationJudge(endPoint.rotation);
  var item = {
    index: numPaths + 1,
    mode: pathName.name,
    velocity: velocity,
    segment: "forward",
    startPoint: { num: numPoints, x: startPoint.x, y: -startPoint.y, rotation: startyaw },
    endPoint: { num: 1, x: endPoint.x, y: -endPoint.y, rotation: endyaw },
    cp1: { x: cp1.x, y: -cp1.y },
    cp2: { x: cp2.x, y: -cp2.y }
  };
  this.curvesObj.push(item);    //将贝塞尔信息加入curvesObj数组
  return pathName;
};
//闭合前进直线文本
ROS2D.PolygonMarker.prototype.createLineEndText = function (startPoint, endPoint, velocity) {
  console.log("createLineEndText");
  var numPaths = this.lineContainer.getNumChildren() / 2;       //路径数量
  var numPoints = this.pointContainer.getNumChildren() / 2;     //点的数量
  var pathName = new createjs.Text(numPaths + 1, "0.2px Arial", this.lineColor);    //创建文本

  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
  pathName.name = "line";

  var startyaw = this.rotationJudge(startPoint.rotation);     //开始角度
  var endyaw = this.rotationJudge(endPoint.rotation);         //结束角度

  var item = {
    index: numPaths + 1,
    mode: pathName.name,
    velocity: velocity,
    segment: "forward",
    startPoint: { num: numPoints, x: startPoint.x, y: -startPoint.y, rotation: startyaw },
    endPoint: { num: 1, x: endPoint.x, y: -endPoint.y, rotation: endyaw },
    cp1: { x: 0, y: 0 },
    cp2: { x: 0, y: 0 }
  };
  this.curvesObj.push(item);      //加入curveObj数组
  return pathName;
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//Create bezier Control points，  贝塞尔控制点获取
ROS2D.PolygonMarker.prototype.createBezierCtpiont = function (startPoint, endPoint, type) {
  console.log("ros2d.js-1003-createBezierCtpiont");
  var cpx;
  var cpy;
  var startyaw = this.rotationJudge(startPoint.rotation);   //开始角度
  var endyaw = this.rotationJudge(endPoint.rotation);       //结束角度
  //贝塞尔曲线长度
  var length = Math.sqrt((startPoint.x - endPoint.x) * (startPoint.x - endPoint.x) + (startPoint.y - endPoint.y) * (startPoint.y - endPoint.y));
  if (type === 0) {     //parseFloat() 函数可解析一个字符串，并返回一个浮点数。
    cpx = parseFloat(startPoint.x) + length * Math.cos(parseFloat(startyaw) * 0.017453293) * 2 / 3;     //1开始控制点
    cpy = parseFloat(startPoint.y) - length * Math.sin(parseFloat(startyaw) * 0.017453293) * 2 / 3;
  } else {
    cpx = parseFloat(endPoint.x) - length * Math.cos(parseFloat(endyaw) * 0.017453293) * 2 / 3;         //0结束控制点
    cpy = parseFloat(endPoint.y) + length * Math.sin(parseFloat(endyaw) * 0.017453293) * 2 / 3;
  }
  return new ROSLIB.Vector3({
    x: cpx,
    y: cpy,
  });
};

//create bezier Text，    创建贝塞尔文本
ROS2D.PolygonMarker.prototype.createBezierText = function (startPoint, endPoint, cp1, cp2, velocity) {
  console.log("ros2d.js-1004-createBezierText");
  var numPoints = this.pointContainer.getNumChildren() / 2;
  console.log("目前点数量 "+numPoints);     //点的数量
  var numPaths = this.lineContainer.getNumChildren() / 2;
  console.log("目前路径数量 "+numPaths);    //线的数量
  var pathName = new createjs.Text(numPaths + 1, "0.2px Arial", "#0000ff");   //已有路径数量+1
  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;     //贝塞尔曲线的x，y坐标值与名字
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
  pathName.name = "bezier";
  var startyaw = this.rotationJudge(startPoint.rotation);
  var endyaw = this.rotationJudge(endPoint.rotation);
  var item = {
    index: numPaths + 1,
    mode: pathName.name,
    velocity: velocity,
    segment: "forward",
    startPoint: { num: numPoints - 1, x: startPoint.x, y: -startPoint.y, rotation: startyaw },
    endPoint: { num: numPoints, x: endPoint.x, y: -endPoint.y, rotation: endyaw },
    cp1: { x: cp1.x, y: -cp1.y },
    cp2: { x: cp2.x, y: -cp2.y }
  };
  this.curvesObj.push(item);    //将贝塞尔信息加入curvesObj数组
  return pathName;
};
//创建贝塞尔控制点形状
ROS2D.PolygonMarker.prototype.createBezierCPShape = function (startPoint, endPoint) {
  console.log("ros2d.js-1007-createBezierCPShape");
  var controlPoint = new createjs.Shape();
  controlPoint.graphics.beginFill(this.pointColor);                          //颜色
  controlPoint.graphics.drawCircle(endPoint.x, -endPoint.y, this.lineSize)   //画圆，圆心的坐标以及半径
  controlPoint.graphics.setStrokeStyle(this.lineSize / 3);                   //设置线条样式，线的粗细
  controlPoint.graphics.setStrokeDash([0.05, 0.05], 0.1);                    //设置线段以虚线的形式显示
  controlPoint.graphics.beginStroke("pink");                                 //指定绘制线条的样式(颜色)
  controlPoint.graphics.moveTo(startPoint.x, -startPoint.y);  //绘制直线，直线的起始点
  controlPoint.graphics.lineTo(endPoint.x, -endPoint.y);      //绘制直线，直线的终点
  this.bezierCPointContainer.addChild(controlPoint);          //添加形状实例
};
//创建贝塞尔曲线形状
ROS2D.PolygonMarker.prototype.createBezierShape = function (startPoint, endPoint, cp1, cp2) {
  console.log("ros2d.js-1000-createBezierShape");
  var bezier = new createjs.Shape();                                //创建一个形状的显示对象
  this.editBezierShape(bezier, startPoint, endPoint, cp1, cp2);     //调用editBezierShape函数
  var that = this;
  bezier.addEventListener('mousedown', function (event) {           //事件监听
    if (that.curveCallBack !== null && typeof that.curveCallBack !== 'undefined') {
      that.curveCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
    }
  });
  return bezier;
};
//编辑贝塞尔形状
ROS2D.PolygonMarker.prototype.editBezierShape = function (bezier, startPoint, endPoint, cp1, cp2) {
  console.log("ros2d.js-1006-editBezierShape");
  bezier.graphics.clear();                                //clear()清除所有绘制命令,重置所有绘制设置
  bezier.graphics.setStrokeStyle(this.lineSize);          //设置线条样式
  bezier.graphics.beginStroke(this.lineColor);            //指定绘制线条的样式(颜色)
  bezier.graphics.moveTo(startPoint.x, startPoint.y);     //绘制直线，直线的起始点
  bezier.graphics.bezierCurveTo(cp1.x, cp1.y, cp2.x, cp2.y, endPoint.x, endPoint.y);      //绘制贝塞尔曲线
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////


/////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//后退直线文本编辑
ROS2D.PolygonMarker.prototype.createBackLineText = function (startPoint, endPoint, velocity, numpath) {
  console.log("createBackLineText");
  var numPaths = this.lineContainer.getNumChildren() / 2;       //路径数量
  var pathName = new createjs.Text(numPaths + 1, "0.2px Arial", this.lineColor);    //创建文本

  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
  pathName.name = "line";

  var startyaw = this.rotationJudge(startPoint.rotation);     //开始角度
  var endyaw = this.rotationJudge(endPoint.rotation);         //结束角度
  
  var pathlist = this.getCurvesObj();
  var item = {
    index: numPaths + 1,
    mode: pathName.name,
    velocity: velocity,
    segment: "backward",
    startPoint: { num: parseInt(numpath)+1, x: startPoint.x, y: startPoint.y, rotation: -startyaw },
    endPoint: { num: parseInt(numpath), x: endPoint.x, y: endPoint.y, rotation: -endyaw },
    cp1: { x: 0, y: 0 },
    cp2: { x: 0, y: 0 }
  };
  this.curvesObj.push(item);      //加入curveObj数组
  return pathName;
};
//创建后退直线形状
ROS2D.PolygonMarker.prototype.createBackLineShape = function (startPoint, endPoint) {
  console.log("createBackLineShape");
  var line = new createjs.Shape();
  this.editBackLineShape(line, startPoint, endPoint);             //编辑直线形状
  var that = this;
  line.addEventListener('mousedown', function (event) {
    if (that.lineCallBack !== null && typeof that.lineCallBack !== 'undefined') {
      that.lineCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
    }
  });
  return line;
};
ROS2D.PolygonMarker.prototype.editBackLineShape = function (line, startPoint, endPoint) {
  console.log("ros2d.js-722-editLineShape");
  line.graphics.clear();
  line.graphics.setStrokeStyle(this.lineSize);
  line.graphics.beginStroke(this.lineColor);
  line.graphics.moveTo(startPoint.x, -startPoint.y);
  line.graphics.lineTo(endPoint.x, -endPoint.y);
};
/////////////////////////////////////////////////////////////////////////////////////////////////////////////////



// create path name   创建直线文本
ROS2D.PolygonMarker.prototype.createLineText = function (startPoint, endPoint, velocity) {
  console.log("ros2d.js-1005-createLineText");
  var numPaths = this.lineContainer.getNumChildren() / 2;       //路径数量
  var numPoints = this.pointContainer.getNumChildren() / 2;     //点的数量
  var pathName = new createjs.Text(numPaths + 1, "0.2px Arial", this.lineColor);    //创建文本

  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
  pathName.name = "line";

  var startyaw = this.rotationJudge(startPoint.rotation);     //开始角度
  var endyaw = this.rotationJudge(endPoint.rotation);         //结束角度

  var item = {
    index: numPaths + 1,
    mode: pathName.name,
    velocity: velocity,
    segment: "forward",
    startPoint: { num: numPoints - 1, x: startPoint.x, y: -startPoint.y, rotation: startyaw },
    endPoint: { num: numPoints, x: endPoint.x, y: -endPoint.y, rotation: endyaw },
    cp1: { x: 0, y: 0 },
    cp2: { x: 0, y: 0 }
  };
  this.curvesObj.push(item);      //加入curveObj数组
  return pathName;
};

//创建直线形状
ROS2D.PolygonMarker.prototype.createLineShape = function (startPoint, endPoint) {
  console.log("ros2d.js-1038-createLineShape");
  var line = new createjs.Shape();
  this.editLineShape(line, startPoint, endPoint);             //编辑直线形状
  var that = this;
  line.addEventListener('mousedown', function (event) {
    if (that.lineCallBack !== null && typeof that.lineCallBack !== 'undefined') {
      that.lineCallBack('mousedown', event, that.lineContainer.getChildIndex(event.target));
    }
  });
  return line;
};
//编辑直线形状
ROS2D.PolygonMarker.prototype.editLineShape = function (line, startPoint, endPoint) {
  console.log("ros2d.js-722-editLineShape");
  line.graphics.clear();
  line.graphics.setStrokeStyle(this.lineSize);
  line.graphics.beginStroke(this.lineColor);
  line.graphics.moveTo(startPoint.x, startPoint.y);
  line.graphics.lineTo(endPoint.x, endPoint.y);
};
// 编辑点的文本
ROS2D.PolygonMarker.prototype.CreatePointText = function (pos) {
  console.log("ros2d.js-1060-CreatePointText");
  var numPoints = this.pointContainer.getNumChildren();
  var pn = new createjs.Text(numPoints / 2 + 1, "0.2px Arial", "#ff4400");
  pn.x = pos.x;
  pn.y = -pos.y + 0.1;
  pn.name = "waypoint";
  return pn;
};

/**
 * Internal use only
 */     //编辑点的形状
ROS2D.PolygonMarker.prototype.createPointShape = function (pos) {
  console.log("ros2d.js-734-createPointShape");
  var point = new createjs.Shape();
  point.graphics.beginFill(this.pointColor);
  point.graphics.drawCircle(0, 0, this.pointSize);
  point.graphics.setStrokeStyle(this.pointSize / 3);
  point.graphics.beginStroke("black");
  point.graphics.moveTo(0, 0);
  point.graphics.lineTo(this.pointSize, 0);
  
  point.x = pos.x;
  point.y = -pos.y;
  point.rotation = this.rotationJudge(pos.rotation);

  var that = this;
  point.addEventListener('mousedown', function (event) {
    if (that.pointCallBack !== null && typeof that.pointCallBack !== 'undefined') {
      that.pointCallBack('mousedown', event, that.pointContainer.getChildIndex(event.target));
    }
  });
  return point;
};

/**
 * Adds a point to the polygon
 *
 * @param position of type ROSLIB.Vector3
 */
ROS2D.PolygonMarker.prototype.addPoint = function (pos) {
  console.log("ros2d.js-757-addPoint");
  var point = this.createPointShape(pos);
  this.pointContainer.addChild(point);
  var numPoints = this.pointContainer.getNumChildren();

  // 0 points -> 1 point, 0 lines
  // 1 point  -> 2 points, lines: add line between previous and new point, add line between new point and first point
  // 2 points -> 3 points, 3 lines: change last line, add line between new point and first point
  // 3 points -> 4 points, 4 lines: change last line, add line between new point and first point
  // etc

  if (numPoints < 2) {
    // Now 1 point
  }
  else if (numPoints < 3) {
    // Now 2 points: add line between previous and new point
    var line = this.createLineShape(this.pointContainer.getChildAt(numPoints - 2), point);
    this.lineContainer.addChild(line);
  }
  
  if (numPoints > 2) {
    // Now 3 or more points: change last line
    this.editLineShape(this.lineContainer.getChildAt(numPoints - 2), this.pointContainer.getChildAt(numPoints - 2), point);
  }
  
  if (numPoints > 1) {
    // Now 2 or more points: add line between new point and first point
    var lineEnd = this.createLineShape(point, this.pointContainer.getChildAt(0));
    this.lineContainer.addChild(lineEnd);
  }
  this.drawFill();
};

//路径增加闭环
ROS2D.PolygonMarker.prototype.addEndLooptPath = function (mode, velocity) {
  console.log("ros2d.js-1010-addEndLooptPath");
  var numPoints = this.pointContainer.getNumChildren() / 2;
  console.log("目前已有的点的数量："+numPoints);         //目前已有的点的数量
  if (numPoints >= 3) {
    var id = numPoints * 2 - 2;         //5点的话，此处id为8
    console.log("numPoints", id);
    
    var startPoint = this.pointContainer.getChildAt(id);    //返回子索引，如果索引处没有子元素，返回null。
    var endPoint = this.pointContainer.getChildAt(0);       //此处endpoint为null
    var numlines = this.curvesObj.length;                   //目前已有的线段数量
    
    var isExist = false;
    for (i = 2; i < numlines; i++) {
      var line = this.curvesObj[i];
      console.log(this.curvesObj[i]);            
      var syaw = this.rotationJudge(startPoint.rotation);
      var eyaw = this.rotationJudge(endPoint.rotation);

      if (line.startPoint.x === startPoint.x && line.startPoint.y === -startPoint.y && line.startPoint.rotation === syaw
        && line.endPoint.x === endPoint.x && line.endPoint.y === -endPoint.y && line.endPoint.rotation === eyaw) {
        console.log("fine curve obj ..");
        if ((mode === 0 && line.mode === "line") || (mode === 1 && line.mode === "bezier")) {     //0为直线，1为贝塞尔曲线
          isExist = true;
          console.log("line exist");
        }
      }
    };
    // Now 2 points: add line between previous and new point
    if (mode === 0 && !isExist) {                                   //mode为0,且isExist为0
      var line = this.createLineShape(startPoint, endPoint);
      var lineLaber = this.createLineEndText(startPoint, endPoint, velocity);
      this.lineContainer.addChild(line, lineLaber);
      console.log("line exist");
    } else if (mode === 1 && !isExist) {
      var cp1 = this.createBezierCtpiont(startPoint, endPoint, 0);
      var cp2 = this.createBezierCtpiont(startPoint, endPoint, 1);
      var bezier = this.createBezierShape(startPoint, endPoint, cp1, cp2);
      var bezierLaber = this.createBezierEndText(startPoint, endPoint, cp1, cp2, velocity);
      this.lineContainer.addChild(bezier, bezierLaber);
      console.log("Bezier exist");
    }
    this.drawFill();
  }
};

// ROS2D.PolygonMarker.prototype.drawEndLooptPath = function (mode, velocity, CtBz1, CtBz2) {
//   console.log("ros2d.js-1010-addEndLooptPath");
//   var numPoints = this.pointContainer.getNumChildren() / 2;
//   console.log("目前已有的点的数量："+numPoints);         //目前已有的点的数量
//   if (numPoints >= 3) {
//     var id = numPoints * 2 - 2;         //5点的话，此处id为8
//     console.log(id);
    
//     var startPoint = this.pointContainer.getChildAt(id);    //返回子索引，如果索引处没有子元素，返回null。
//     var endPoint = this.pointContainer.getChildAt(0);       //此处endpoint为null
//     var numlines = this.curvesObj.length;                   //目前已有的线段数量
    
//     var isExist = false;
//     for (i = 2; i < numlines; i++) {
//       var line = this.curvesObj[i];
//       console.log(this.curvesObj[i]);            
//       var syaw = this.rotationJudge(startPoint.rotation);
//       var eyaw = this.rotationJudge(endPoint.rotation);

//       if (line.startPoint.x === startPoint.x && line.startPoint.y === -startPoint.y && line.startPoint.rotation === syaw
//         && line.endPoint.x === endPoint.x && line.endPoint.y === -endPoint.y && line.endPoint.rotation === eyaw) {
//         console.log("fine curve obj ..");
//         if ((mode === 0 && line.mode === "line") || (mode === 1 && line.mode === "bezier")) {     //0为直线，1为贝塞尔曲线
//           isExist = true;
//           console.log("line exist");
//         }
//       }
//     }
//     // Now 2 points: add line between previous and new point
//     if (mode === 0 && !isExist) {                                   //mode为0,且isExist为0
//       var line = this.createLineShape(startPoint, endPoint);
//       var lineLaber = this.createLineText(startPoint, endPoint, velocity);
//       this.lineContainer.addChild(line, lineLaber);
//       console.log("line exist");
//     } else if (mode === 1 && !isExist) {
//       var cp1 = CtBz1;
//       var cp2 = CtBz2;
//       var bezier = this.createBezierShape(startPoint, endPoint, cp1, cp2);
//       var bezierLaber = this.createBezierText(startPoint, endPoint, cp1, cp2, velocity);
//       this.lineContainer.addChild(bezier, bezierLaber);
//       console.log("Bezier exist");
//     }
//     this.drawFill();
//   }
// };



//地图软件绘制函数集
///////////////////////////////////////////////////////////////////////////////////
ROS2D.PolygonMarker.prototype.drawPoint = function (pos, pos_name) {
  console.log("ros2d.js-1230-drawPoint");
  var point = this.createPointShape(pos);
  var pointLaber = this.drawPointText(pos, pos_name);
  this.pointContainer.addChild(point, pointLaber);
  var numPoints = this.pointContainer.getNumChildren() / 2;   //当前点的数量
  console.log("当前点的数量："+numPoints);
};
ROS2D.PolygonMarker.prototype.drawPointText = function (pos, pos_name) {
  console.log("ros2d.js-1238-drawPointText");
  var pn = new createjs.Text(pos_name, "0.2px Arial", "#ff4400");
  pn.x = pos.x;
  pn.y = -pos.y + 0.1;
  pn.name = pos_name;
  return pn;
};
ROS2D.PolygonMarker.prototype.drawLineText = function (startPoint, endPoint, velocity, numpath, startPos, endPos, mode) {
  console.log("ros2d.js-1246-drawLineText");
  var numPaths = this.lineContainer.getNumChildren() / 2;
  // console.log("线段容器中的数量："+numPaths);
  var direction = "forward";
  if (mode == 1) {
    direction = "backward";
  };
  var pathName = new createjs.Text(numpath, "0.2px Arial", this.lineColor);    //创建文本
  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5 + (mode == 1 ? 0.15 : 0);
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5 + (mode == 1 ? 0.15 : 0);
  pathName.name = "line";
  var startyaw = this.rotationJudge(startPoint.rotation);     //开始角度
  var endyaw = this.rotationJudge(endPoint.rotation);         //结束角度
  var item = {
    index: numpath,
    mode: pathName.name,
    velocity: velocity,
    segment: direction,
    startPoint: { num: parseInt(startPos), x: startPoint.x, y: startPoint.y, rotation: -startyaw },
    endPoint: { num: parseInt(endPos), x: endPoint.x, y: endPoint.y, rotation: -endyaw },
    cp1: { x: 0, y: 0 },
    cp2: { x: 0, y: 0 }
  };
  // console.log(item);
  this.curvesObj.push(item);      //加入curveObj数组
  return pathName;
};


ROS2D.PolygonMarker.prototype.drawBezierText = function (startPoint, endPoint, cp1, cp2, velocity, numpath, startPos, endPos, mode) {
  console.log("ros2d.js-1273-drawBezierText");
  // var numPoints = this.pointContainer.getNumChildren() / 2;
  // var numPaths = this.lineContainer.getNumChildren() / 2;
  var direction = "forward";
  var pathName = new createjs.Text(numpath, "0.2px Arial", this.lineColor);   //已有路径数量+1
  if (mode == 1) {
    direction = "backward";
    //pathName = new createjs.Text(numpath, "0.2px Arial", "##00ff00");
  };
  
  pathName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5 + (mode == 1 ? 0.15 : 0) ;     //贝塞尔曲线的x，y坐标值与名字
  pathName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5 + (mode == 1 ? 0.15 : 0);
  pathName.name = "bezier";
  
  var startyaw = this.rotationJudge(startPoint.rotation);
  var endyaw = this.rotationJudge(endPoint.rotation);
  var item = {
    index: numpath,
    mode: pathName.name,
    velocity: velocity,
    segment: direction,
    startPoint: { num: parseInt(startPos), x: startPoint.x, y: -startPoint.y, rotation: startyaw },
    endPoint: { num: parseInt(endPos), x: endPoint.x, y: -endPoint.y, rotation: endyaw },
    cp1: { x: cp1.x, y: -cp1.y },
    cp2: { x: cp2.x, y: -cp2.y }
  };
  
  this.curvesObj.push(item);    //将贝塞尔信息加入curvesObj数组
  return pathName;
};

ROS2D.PolygonMarker.prototype.addPointPath = function (pos, mode, velocity) {
  console.log("ros2d.js-790-addPointPath");
  if (mode < 100) {
    var point = this.createPointShape(pos);
    var pointLaber = this.CreatePointText(pos);
    this.pointContainer.addChild(point, pointLaber);
    var numPoints = this.pointContainer.getNumChildren() / 2;   //当前点的数量
    console.log("当前点的数量："+numPoints);

    if (numPoints > 1) {
      var id = numPoints * 2 - 4;
      var startPoint = this.pointContainer.getChildAt(id);
      // Now 2 points: add line between previous and new point
      if (mode === 0) {
        console.log("直线");                                                     //0是直线
        var line = this.createLineShape(startPoint, point);
        var lineLaber = this.createLineText(startPoint, point, velocity);
        this.lineContainer.addChild(line, lineLaber);
      } else if (mode === 1) { 
        console.log("曲线");                                             //1为贝塞尔曲线
        var cp1 = this.createBezierCtpiont(startPoint, point, 0);
        var cp2 = this.createBezierCtpiont(startPoint, point, 1);
        var bezier = this.createBezierShape(startPoint, point, cp1, cp2);
        var bezierLaber = this.createBezierText(startPoint, point, cp1, cp2, velocity);
        this.lineContainer.addChild(bezier, bezierLaber);
      }
    }
    this.drawFill();
  }
};

//配合地图绘制软件函数
// ROS2D.PolygonMarker.prototype.drawPointPath = function (pos, mode, velocity, CtBz1, CtBz2) {
//   console.log("drawPointPath");
//   if (mode < 100) {
//     var point = this.createPointShape(pos);
//     var pointLaber = this.CreatePointText(pos);
//     this.pointContainer.addChild(point, pointLaber);
//     var numPoints = this.pointContainer.getNumChildren() / 2;   //当前点的数量
//     console.log("当前点的数量："+numPoints);

//     if (numPoints > 1) {
//       var id = numPoints * 2 - 4;
//       var startPoint = this.pointContainer.getChildAt(id);
//       // Now 2 points: add line between previous and new point
//       if (mode === 0) {
//         console.log("直线");                                                     //0是直线
//         var line = this.createLineShape(startPoint, point);
//         var lineLaber = this.createLineText(startPoint, point, velocity);
//         this.lineContainer.addChild(line, lineLaber);
//       } else if (mode === 1) { 
//         console.log("曲线");                                             //1为贝塞尔曲线
//         var cp1 = CtBz1;
//         var cp2 = CtBz2;
//         console.log(cp1, cp2);
//         var bezier = this.createBezierShape(startPoint, point, cp1, cp2);
//         var bezierLaber = this.createBezierText(startPoint, point, cp1, cp2, velocity);
//         this.lineContainer.addChild(bezier, bezierLaber);
//       }
//     }
//     this.drawFill();
//   }
// };



/**
 * Removes a point from the polygon
 *
 * @param obj either an index (integer) or a point shape of the polygon
 */
ROS2D.PolygonMarker.prototype.remPoint = function (obj) {   // 对象索引， obj是一个索引(整数)或多边形的点形状。
  console.log("ros2d.js-795-remPoint");
  var index;
  //	var point;
  if (obj instanceof createjs.Shape) {
    index = this.pointContainer.getChildIndex(obj);
    //point = obj;
  }
  else {
    index = obj;
    //point = this.pointContainer.getChildAt(index);
  }
  var numPoints = this.pointContainer.getNumChildren() / 2;
  if (numPoints < 2) {

  }
  else if (numPoints < 3) {
    // 2 points: remove all lines
    this.lineContainer.removeAllChildren();
    this.curvesObj = [];
  }
  else {
    // 3 or more points: change line before point to remove, remove line after point to remove
    var pointName = this.pointContainer.getChildAt(index + 1)
    var numPaths = this.lineContainer.getNumChildren();
    // console.log("befor",this.curvesObj);
    for (i = 0; i < numPaths;) {
      var lineName = this.lineContainer.getChildAt(i + 1);
      //parseInt() 函数解析字符串并返回整数。
      if (parseInt(lineName.text) == parseInt(pointName.text) || parseInt(lineName.text) + 1 == parseInt(pointName.text)) {
        this.lineContainer.removeChildAt(i);
        this.lineContainer.removeChildAt(i);
        numPaths = numPaths - 2;
        for (n = 0; n < this.curvesObj.length; n++) {
          var id = this.curvesObj[n].index;
          if (parseInt(lineName.text) === id) {
            this.curvesObj.splice(n, 1);
          }
        }
        i = 0;
        continue;
      }
      i = i + 2;
    }
    // console.log("affter",this.curvesObj);
  }
  this.pointContainer.removeChildAt(index);
  this.pointContainer.removeChildAt(index);
  //	this.points.splice(index, 1);
  this.drawFill();
};

ROS2D.PolygonMarker.prototype.removeAllPoint = function (obj) {
  console.log("ros2d.js-1401-removeAllPoint");
  var index;
  //	var point;
  if (obj instanceof createjs.Shape) {
    index = this.pointContainer.getChildIndex(obj);
    //		point = obj;
  }
  else {
    index = obj;
    //		point = this.pointContainer.getChildAt(index);
  }
  
  var numPoints = this.pointContainer.getNumChildren();
  console.log("ros2d.js-1414-numPoints: " + numPoints);
  this.lineContainer.removeAllChildren();             //从显示列表中删除移除所有子元素
  this.pointContainer.removeAllChildren();
  this.bezierCPointContainer.removeAllChildren();
  this.curvesObj = [];
  this.drawFill();
};

/**
 * Moves a point of the polygon
 *
 * @param obj either an index (integer) or a point shape of the polygon
 * @param position of type ROSLIB.Vector3
 */
ROS2D.PolygonMarker.prototype.movePoint = function (obj, newPos) {
  console.log("ros2d.js-845-movePoint");
  var index;
  var point;
  var pointName;

  if (obj instanceof createjs.Shape) {                //instanceof :判断一个对象是否为一个类（或接口、抽象类、父类）的实例
    index = this.pointContainer.getChildIndex(obj);   //返回显示列表中指定子元素的索引，如果不在显示列表中，则返回-1。
    point = obj;
  }
  else {
    index = obj;                                            //得到移动点的索引
    console.log("移动点的索引:" + index);                     //以点2为例，index 2
    point = this.pointContainer.getChildAt(index);          //返回位于指定索引处的子节点。
    pointName = this.pointContainer.getChildAt(index + 1);  //移动点的名称
  }
  
  point.x = newPos.x;           //此处坐标转换
  point.y = -newPos.y;
  pointName.x = point.x;
  pointName.y = point.y + 0.1;
  
  var numPoints = this.pointContainer.getNumChildren() / 2;
  console.log("目前已有的点数：" + numPoints);     //现有的点数
  if (numPoints > 1) {
    var pointName = this.pointContainer.getChildAt(index + 1);
    var numPaths = this.lineContainer.getNumChildren();
    console.log("路径数量：" + numPaths);         //路径数量的两倍

    for (i = 0; i < numPaths;) {
      console.log(i);
      var lineName = this.lineContainer.getChildAt(i + 1);
      console.log("线段遍历：" + lineName.text);            //线段名称，此处为1
      console.log("移动点的索引：" + pointName.text);        //点的名称，此处为2
      
      // 检查路径索引与点的索引，找到点的前一段路径与后一段路径。   移动点不是第一点
      if ((parseInt(lineName.text) + 1 == parseInt(pointName.text) || parseInt(lineName.text) == parseInt(pointName.text)) && (parseInt(pointName.text) !== 1)) {
        var num = 0;
        var path = this.lineContainer.getChildAt(i);
        var startPoint;
        var endPoint;
        if (parseInt(lineName.text) == parseInt(pointName.text)) {
          endPoint = this.pointContainer.getChildAt((index + 2 + numPoints * 2) % (numPoints * 2)); // second line
          startPoint = point;
          console.log("second line");
        }
        else if(parseInt(lineName.text) + 1 == parseInt(pointName.text)){
          endPoint = point;
          startPoint = this.pointContainer.getChildAt((index - 2 + numPoints * 2) % (numPoints * 2)); // first line
          console.log("first line");
        }
        
        //find curves obj
        console.log("循环查询路径的索引")
        for (n = 0; n < this.curvesObj.length; n++) {
          var id = this.curvesObj[n].index;
          if (parseInt(lineName.text) === id) {
            num = n;
            break;
          };
        };

        if (lineName.name === "line") {
          console.log("直线绘制")
          this.editLineShape(path, startPoint, endPoint);
        } else {
          var cp1 = this.createBezierCtpiont(startPoint, endPoint, 0);
          var cp2 = this.createBezierCtpiont(startPoint, endPoint, 1);
          this.editBezierShape(path, startPoint, endPoint, cp1, cp2);
          this.curvesObj[num].cp1.x = cp1.x;
          this.curvesObj[num].cp1.y = -cp1.y;
          this.curvesObj[num].cp2.x = cp2.x;
          this.curvesObj[num].cp2.y = -cp2.y;
          console.log(this.curvesObj[num]);
        }
        lineName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;
        lineName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
        this.curvesObj[num].startPoint.x = startPoint.x;
        this.curvesObj[num].startPoint.y = -startPoint.y;
        this.curvesObj[num].endPoint.x = endPoint.x;
        this.curvesObj[num].endPoint.y = -endPoint.y;
        console.log(this.curvesObj[num]);           //对应路线的信息
      }else if(((parseInt(lineName.text) == parseInt(this.curvesObj.length)) || parseInt(lineName.text) == parseInt(pointName.text)) && (parseInt(pointName.text) == 1)){
        console.log("移动第一点")
        var num = 0;
        var path = this.lineContainer.getChildAt(i);
        var startPoint;
        var endPoint;

        if (parseInt(lineName.text) == parseInt(pointName.text)) {
          endPoint = this.pointContainer.getChildAt((index + 2 + numPoints * 2) % (numPoints * 2)); // second line
          startPoint = point;
          console.log("second line");
        }else{
          endPoint = point;
          startPoint = this.pointContainer.getChildAt(numPoints*2-2);
          console.log("last line");
        }

        for (n = 0; n < this.curvesObj.length; n++) {
          var id = this.curvesObj[n].index;
          console.log(id);                                    //此处为1
          if (parseInt(lineName.text) === id) {
            console.log(parseInt(lineName.text));             //此处为1
            num = n;
            break;
          }
        }
        if (lineName.name === "line") {
          console.log(lineName.name);
          this.editLineShape(path, startPoint, endPoint);
        } else {
          var cp1 = this.createBezierCtpiont(startPoint, endPoint, 0);
          var cp2 = this.createBezierCtpiont(startPoint, endPoint, 1);
          this.editBezierShape(path, startPoint, endPoint, cp1, cp2);
          this.curvesObj[num].cp1.x = cp1.x;
          this.curvesObj[num].cp1.y = -cp1.y;
          this.curvesObj[num].cp2.x = cp2.x;
          this.curvesObj[num].cp2.y = -cp2.y;
          console.log(this.curvesObj[num]);
        }
        lineName.x = (parseFloat(startPoint.x) + parseFloat(endPoint.x)) * 0.5;
        lineName.y = (parseFloat(startPoint.y) + parseFloat(endPoint.y)) * 0.5;
        this.curvesObj[num].startPoint.x = startPoint.x;
        this.curvesObj[num].startPoint.y = -startPoint.y;
        this.curvesObj[num].endPoint.x = endPoint.x;
        this.curvesObj[num].endPoint.y = -endPoint.y;
        console.log(this.curvesObj[num]);           //对应路线的信息
      };
      i = i + 2;
      //isEndPath = isEndPath-1;
    }
  }
  this.drawFill();
};

/**
 * Splits a line of the polygon: inserts a point at the center of the line
 *
 * @param obj either an index (integer) or a line shape of the polygon
 */
ROS2D.PolygonMarker.prototype.splitLine = function (obj) {
  //console.log("ros2d.js-879-splitLine");
  var index;
  var line;
  if (obj instanceof createjs.Shape) {
    index = this.lineContainer.getChildIndex(obj);
    line = obj;
  }
  else {
    index = obj;
    line = this.lineContainer.getChildAt(index);
  }
  var numPoints = this.pointContainer.getNumChildren();
  var xs = this.pointContainer.getChildAt(index).x;
  var ys = this.pointContainer.getChildAt(index).y;
  var xe = this.pointContainer.getChildAt((index + 1) % numPoints).x;
  var ye = this.pointContainer.getChildAt((index + 1) % numPoints).y;
  var xh = (xs + xe) / 2.0;
  var yh = (ys + ye) / 2.0;
  var pos = new ROSLIB.Vector3({ x: xh, y: -yh });

  // Add a point in the center of the line to split
  var point = this.createPointShape(pos);
  this.pointContainer.addChildAt(point, index + 1);
  ++numPoints;

  // Add a line between the new point and the end of the line to split
  var lineNew = this.createLineShape(point, this.pointContainer.getChildAt((index + 2) % numPoints));
  this.lineContainer.addChildAt(lineNew, index + 1);

  // Set the endpoint of the line to split to the new point
  this.editLineShape(line, this.pointContainer.getChildAt(index), point);

  this.drawFill();
};

/**
 * Internal use only
 */
ROS2D.PolygonMarker.prototype.drawFill = function () {
  console.log("ros2d.js-918-drawFill");
  var numPoints = this.pointContainer.getNumChildren();
  if (numPoints > 2) {
    var g = this.fillShape.graphics;
    g.clear();
    g.setStrokeStyle(0);
    g.moveTo(this.pointContainer.getChildAt(0).x, this.pointContainer.getChildAt(0).y);
    g.beginStroke();
    g.beginFill(this.fillColor);
    for (var i = 1; i < numPoints; ++i) {
      g.lineTo(this.pointContainer.getChildAt(i).x, this.pointContainer.getChildAt(i).y);
    }
    g.closePath();
    g.endFill();
    g.endStroke();
  }
  else {
    this.fillShape.graphics.clear();
  }
};
ROS2D.PolygonMarker.prototype.__proto__ = createjs.Container.prototype;


////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * A Peripheral Device of poses,
 *
 * @constructor
 * @param options - object with following keys:
 *   * hight (optional) - the device hight
 *   * width (optional) - the device width
 *   * angle (optional) - the device angle
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 */
ROS2D.PeripheralShape = function (options) {
  options = options || {};
  this.height = options.height || 20;   // 设置默认高度为20
  this.width = options.width || 10;     // 添加宽度属性并设置默认值为10
  this.angle = options.angle || 0;
  this.state = options.state || 0;
  this.strokeSize = options.strokeSize || 3;
  this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 255);
  // var fillColor = options.fillColor || createjs.Graphics.getRGB(0, 0, 255);

  // Create the graphics
  this.graphics = new createjs.Graphics();
  this.graphics.setStrokeStyle(this.strokeSize);
  this.graphics.beginStroke(this.strokeColor);
  
  this.graphics.drawCircle(this.width/2, 0, 0.2); // draw rotations
  this.graphics.drawRoundRect(-this.width/2, -this.height/2, this.width, this.height, 0.5); // 使用Rect绘制矩形
  this.graphics.endStroke();

  // Create the shape
  createjs.Shape.call(this, this.graphics);
};

ROS2D.PeripheralShape.prototype.rotationAngle = function (angle) {
  this.angle = angle;
}

ROS2D.PeripheralShape.prototype.updateStatus = function(state) {
  
  if (this.state != state) {
    this.graphics.clear();
    var fillColor;
    if (state) {
      fillColor = createjs.Graphics.getRGB(255, 0, 0, 0.4);
      this.graphics.beginFill(fillColor); // 开始填充并设置填充颜色
    }
    
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
  
    this.graphics.drawCircle(this.width/2, 0, 0.2); // draw rotations
    this.graphics.drawRoundRect(-this.width/2, -this.height/2, this.width, this.height, 0.5); // 使用Rect绘制矩形
    this.graphics.endStroke();
    
    if (state)
      this.graphics.endFill();
  }

  this.state = state;
}

ROS2D.PeripheralShape.prototype.__proto__ = createjs.Shape.prototype;

/**
 * A trace of poses, handy to see where a robot has been
 *
 * @constructor
 * @param options - object with following keys:
 *   * pose (optional) - the first pose of the trace
 *   * strokeSize (optional) - the size of the outline
 *   * strokeColor (optional) - the createjs color for the stroke
 *   * maxPoses (optional) - the maximum number of poses to keep, 0 for infinite
 *   * minDist (optional) - the minimal distance between poses to use the pose for drawing (default 0.05)
 */
ROS2D.TraceShape = function (options) {
  //console.log("ros2d.js-958-TraceShape");
  //var that = this;
  options = options || {};
  var pose = options.pose;
  this.strokeSize = options.strokeSize || 3;
  this.strokeColor = options.strokeColor || createjs.Graphics.getRGB(0, 0, 0);
  this.maxPoses = options.maxPoses || 100;
  this.minDist = options.minDist || 0.02;

  // Store minDist as the square of it
  this.minDist = this.minDist * this.minDist;

  // Array of the poses
  // TODO: do we need this?
  this.poses = [];

  // Create the graphics
  this.graphics = new createjs.Graphics();
  this.graphics.setStrokeStyle(this.strokeSize);
  this.graphics.beginStroke(this.strokeColor);

  // Add first pose if given
  if (pose !== null && typeof pose !== 'undefined') {
    this.poses.push(pose);
  }

  // Create the shape
  createjs.Shape.call(this, this.graphics);
};

/**
 * Adds a pose to the trace and updates the graphics
 *
 * @param pose of type ROSLIB.Pose
 */
ROS2D.TraceShape.prototype.addPose = function (pose) {
  // console.log("ros2d.js-994-addPose");
  var last = this.poses.length - 1;
 
  if (last < 0) {
    this.poses.push(pose);
    this.graphics.moveTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
  } else {
    var dx = (pose.position.x - this.poses[last].position.x);
    var dy = (pose.position.y - this.poses[last].position.y);
    var dis = dx * dx + dy * dy;
    
    // if (dis > this.minDist) {
      this.graphics.lineTo(pose.position.x / this.scaleX, pose.position.y / -this.scaleY);
      this.poses.push(pose);
    // }
  }

  if (this.maxPoses > 0 && this.maxPoses < this.poses.length) {
    this.popFront();
  }
};

/**
 * Removes front pose and updates the graphics
 */
ROS2D.TraceShape.prototype.popFront = function () {
  // console.log("ros2d.js-1019-popFront");
  if (this.poses.length > 0) {
    this.poses.shift();
    // TODO: shift drawing instructions rather than doing it all over
    this.graphics.clear();
    this.graphics.setStrokeStyle(this.strokeSize);
    this.graphics.beginStroke(this.strokeColor);
    this.graphics.lineTo(this.poses[0].position.x / this.scaleX, this.poses[0].position.y / -this.scaleY);
    for (var i = 1; i < this.poses.length; ++i) {
      this.graphics.lineTo(this.poses[i].position.x / this.scaleX, this.poses[i].position.y / -this.scaleY);
    }
  }
};
ROS2D.TraceShape.prototype.__proto__ = createjs.Shape.prototype;

/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * Adds panning to a view
 *
 * @constructor
 * @param options - object with following keys:
 *   * rootObject (optional) - the root object to apply panning to
 */
ROS2D.PanView = function (options) {
  //console.log("ros2d.js-1047-PanView");
  options = options || {};
  this.rootObject = options.rootObject;

  // get a handle to the stage
  if (this.rootObject instanceof createjs.Stage) {
    this.stage = this.rootObject;
  }
  else {
    this.stage = this.rootObject.getStage();
  }

  this.startPos = new ROSLIB.Vector3();
};


ROS2D.PanView.prototype.startPan = function (startX, startY) {
  //console.log("ros2d.js-1064-startPan");
  this.startPos.x = startX;
  this.startPos.y = startY;
};

ROS2D.PanView.prototype.pan = function (curX, curY) {
  //console.log("ros2d.js-1070-pan");
  this.stage.x += curX - this.startPos.x;
  this.startPos.x = curX;
  this.stage.y += curY - this.startPos.y;
  this.startPos.y = curY;
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Russell Toris - rctoris@wpi.edu
 */

/**
 * A Viewer can be used to render an interactive 2D scene to a HTML5 canvas.
 *
 * @constructor
 * @param options - object with following keys:
 *   * divID - the ID of the div to place the viewer in
 *   * width - the initial width, in pixels, of the canvas
 *   * height - the initial height, in pixels, of the canvas
 *   * background (optional) - the color to render the background, like '#efefef'
 */
ROS2D.Viewer = function (options) {
  //console.log("ros2d.js-1092-Viewer");
  var that = this;
  options = options || {};
  var divID = options.divID;
  this.width = options.width;
  this.height = options.height;
  var background = options.background || '#111111';

  // create the canvas to render to
  var canvas = document.createElement('canvas');
  canvas.width = this.width;
  canvas.height = this.height;
  canvas.style.background = background;
  document.getElementById(divID).appendChild(canvas);
  // create the easel to use
  this.scene = new createjs.Stage(canvas);

  // change Y axis center
  this.scene.y = this.height;

  // add the renderer to the page
  document.getElementById(divID).appendChild(canvas);

  // update at 30fps
  createjs.Ticker.setFPS(30);
  createjs.Ticker.addEventListener('tick', this.scene);
};

/**
 * Add the given createjs object to the global scene in the viewer.
 *
 * @param object - the object to add
 */
ROS2D.Viewer.prototype.addObject = function (object) {
  //console.log("ros2d.js-1126-addObject");
  this.scene.addChild(object);
};

/**
 * Scale the scene to fit the given width and height into the current canvas.
 *
 * @param width - the width to scale to in meters
 * @param height - the height to scale to in meters
 */
ROS2D.Viewer.prototype.scaleToDimensions = function (width, height) {
  //console.log("ros2d.js-1186-scaleToDimensions");
  //console.log("ros2d.js-1187-Map width: " + width + " (m)");
  //console.log("ros2d.js-1188-Map height: " + height + " (m)");

  // restore to values before shifting, if ocurred
  this.scene.x = typeof this.scene.x_prev_shift !== 'undefined' ? this.scene.x_prev_shift : this.scene.x;
  this.scene.y = typeof this.scene.y_prev_shift !== 'undefined' ? this.scene.y_prev_shift : this.scene.y;

  // save scene scaling
  this.scene.scaleX = this.width / width;
  this.scene.scaleY = this.height / height;
  //console.log("ros2d.js-1194-scaleX: " + this.scene.scaleX);
  //console.log("ros2d.js-1194-scaleY: " + this.scene.scaleY);
};

/**
 * Shift the main view of the canvas by the given amount. This is based on the
 * ROS coordinate system. That is, Y is opposite that of a traditional canvas.
 *
 * @param x - the amount to shift by in the x direction in meters
 * @param y - the amount to shift by in the y direction in meters
 */
ROS2D.Viewer.prototype.shift = function (x, y) {
  //console.log("ros2d.js-1204-shift");
  //console.log("ros2d.js-1205-shift X: " + x);
  //console.log("ros2d.js-1206-shift Y: " + y);

  // save current offset
  this.scene.x_prev_shift = this.scene.x;
  this.scene.y_prev_shift = this.scene.y;

  // shift scene by scaling the desired offset
  this.scene.x -= (x * this.scene.scaleX);
  this.scene.y += (y * this.scene.scaleY);
};
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/**
 * @author Bart van Vliet - bart@dobots.nl
 */

/**
 * Adds zooming to a view
 *
 * @constructor
 * @param options - object with following keys:
 *   * rootObject (optional) - the root object to apply zoom to
 *   * minScale (optional) - minimum scale to set to preserve precision
 */
ROS2D.ZoomView = function (options) {
  //console.log("ros2d.js-1178-ZoomView");
  options = options || {};
  this.rootObject = options.rootObject;
  this.minScale = options.minScale || 0.001;

  // get a handle to the stage
  if (this.rootObject instanceof createjs.Stage) {
    this.stage = this.rootObject;
  }
  else {
    this.stage = this.rootObject.getStage();
  }

  this.center = new ROSLIB.Vector3();
  this.startShift = new ROSLIB.Vector3();
  this.startScale = new ROSLIB.Vector3();
};


ROS2D.ZoomView.prototype.startZoom = function (centerX, centerY) {
  //console.log("ros2d.js-1198-startZoom");
  this.center.x = centerX;
  this.center.y = centerY;
  this.startShift.x = this.stage.x;
  this.startShift.y = this.stage.y;
  this.startScale.x = this.stage.scaleX;
  this.startScale.y = this.stage.scaleY;
};

ROS2D.ZoomView.prototype.zoom = function (zoom) {
  //console.log("ros2d.js-1208-zoom");
  //Make sure scale doesn't become too small
  if (this.startScale.x * zoom < this.minScale) {
    zoom = this.minScale / this.startScale.x;
  }
  if (this.startScale.y * zoom < this.minScale) {
    zoom = this.minScale / this.startScale.y;
  }

  this.stage.scaleX = this.startScale.x * zoom;
  this.stage.scaleY = this.startScale.y * zoom;
  //console.log("ros2d.js-1284-this.stage.scaleX: " + this.stage.scaleX);
  //console.log("ros2d.js-1285-this.stage.scaleY: " + this.stage.scaleY);

  this.stage.x = this.startShift.x - (this.center.x - this.startShift.x) * (this.stage.scaleX / this.startScale.x - 1);
  this.stage.y = this.startShift.y - (this.center.y - this.startShift.y) * (this.stage.scaleY / this.startScale.y - 1);
  //console.log("ros2d.js-1289-this.stage.x: " + this.stage.x);
  //console.log("ros2d.js-1290-this.stage.y: " + this.stage.y);
};

